import java.util.ArrayList;

// Copyright 2006-2013 Dr. Marc Andreas Freese. All rights reserved. 
// marc@coppeliarobotics.com
// www.coppeliarobotics.com
// 
// -------------------------------------------------------------------
// This file is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
// 
// You are free to use/modify/distribute this file for whatever purpose!
// -------------------------------------------------------------------
//
// This file was automatically created for V-REP release V3.0.3 on April 29th 2013

// Make sure to have the server side running in V-REP!
// Start the server from a child script with following command:
// simExtRemoteApiStart(19999) -- starts a remote API server service on port 19999

public class simpleTest
{
	public static void main(String[] args)
	{
		System.out.println("Program started");
		//System.setProperty( "java.library.path", "D:\\Program Files\\vrep\\remoteApiJava.dll");
		System.out.println("JVM Bit size: " + System.getProperty("sun.arch.data.model"));
		remoteApi vrep = new remoteApi();		
		int clientID = vrep.simxStart("127.0.0.1",21000,true,true,5000,5);
		if (clientID!=-1)
		{
			System.out.println("Connected to remote API server");
			IntWA objectHandles = new IntWA(1);
			
			
			
			
			
			/*Used to get the number of objects in a scene*/
			
			//int ret=vrep.simxGetObjects(clientID,vrep.sim_handle_all,objectHandles,vrep.simx_opmode_oneshot_wait);
//			if (ret==vrep.simx_error_noerror)
//				System.out.format("Number of objects in the scene: %d\n",objectHandles.getArray().length);
//			else
//				System.out.format("Remote API function call returned with error code: %d\n",ret);
			//vrep.simxFinish(clientID);
			
			
			
			/*A help class. Used in for example set object positions where fw will contains x,y and z.*/
			//FloatWA fw = new FloatWA(3);
			//fw.w[0] = 1;
			//fw.w[1] = 1;
			//fw.w[2] = 1;
			IntW obectHandle = new IntW(-1);
			//Prismatic_joint
			
			/*Gets tje handler of an specific object, in this case for the HeadYaw joint*/
			int objectHandle = vrep.simxGetObjectHandle(clientID,"HeadYaw",obectHandle,vrep.simx_opmode_oneshot_wait);
			
			
			//int a = vrep.simxRemoveObject(clientID, obectHandle.w, vrep.simx_opmode_oneshot );
			//System.out.println(a);
			System.out.println(objectHandle);
			
			/*This gets all the joints in the current scene. That is if we only have the NAO robot, we will get all the joints for it.
			 * The object handles is then stored in the helper class IntWA called joints and later in an arraylist.*/
			IntWA joints = new IntWA(50);
			vrep.simxGetObjects(clientID, vrep.sim_object_joint_type,joints, vrep.simx_opmode_oneshot_wait );
			ArrayList<Integer> jointsHandlers = new ArrayList<Integer>();
			for(int i:joints.w){
				if(i > 0){
					jointsHandlers.add(i);
					System.out.println(i);
				}
			}
			int counter = 0;
			
			
			
			
			//This is bassicly to make it move backwards and forward. You will get it when you try the robot. Currently the speed is set to -0.4 and 0.4.
			while (vrep.simxGetConnectionId(clientID)!=-1) // while we are connected to the server..
			{ 
				int a = vrep.simxSetJointTargetPosition( clientID, jointsHandlers.get(1),-30,vrep.simx_opmode_oneshot);
				//vrep.simxSetJointTargetVelocity( clientID, 16,100,vrep.simx_opmode_oneshot);
				
//				float value = (float) -0.1;
//				if(counter <= 1500){
//					value = (float) 0.1;
//					
//				}else if(counter == 3000){
//					counter = 0;
//				}
//				counter++;
//				
//				/* Pauses the communications. This cases all the happens untill it is resumed to happen at the same time. 
//				 * For example we sett the speed/positions to all the joints at the same time.
//				 */
//				
//				vrep.simxPauseCommunication(clientID,true);
////				vrep.simxSetJointPosition(clientID,joint1Handle,joint1Value,simx_opmode_oneshot);
////				vrep.simxSetJointPosition(clientID,joint2Handle,joint2Value,simx_opmode_oneshot);
////				vrep.simxSetJointPosition(clientID,joint3Handle,joint3Value,simx_opmode_oneshot);
//				int count = 0;
//				int count2 = 0;
//				
//				
//				
//				//FloatWA angles = new FloatWA(3);
//				//angles.w[0] = (float) 3;   //(float) fw.w[0];
//				//angles.w[1] = (float) 3;//(float) fw.w[1];
//				//angles.w[2] = (float) 3;//(float) fw.w[2] ;
//			   // if(vrep.simxSetObjectPosition (clientID,obectHandle.w,-1,angles,vrep.simx_opmode_oneshot ) == vrep.simx_error_noerror){/			    	//System.out.println("Success!");
//				
//				
//				/* This I do not understand. Comment out set position, and it does nothing. Have both uncommented it moves. Comment out the velocity it still moves, but differently!!
//				 * 
//				 * */	
//				 
//				for(int i :jointsHandlers){
//					int a = vrep.simxSetJointTargetPosition( clientID, i,value,vrep.simx_opmode_oneshot);
//					
//					//int x = vrep.simxSetJointPosition( clientID, i,value,vrep.simx_opmode_oneshot);
//					int b = vrep.simxSetJointTargetVelocity( clientID, i,-value,vrep.simx_opmode_oneshot);
//					if(count == 19)
//						value = -value;
//					//int b = vrep.simxSetJointPosition( clientID, i,(float)5,vrep.simx_opmode_oneshot);
//					
//					//vrep.simxSetObjectPosition (clientID,i,-1,angles,vrep.simx_opmode_oneshot );
////					if(a == vrep.simx_error_noerror){
////						count++;
////					}
////					if(b == vrep.simx_error_noerror){
////						count2++;
////					}
//				}
//				
//				//vrep.simxSetJointTargetPosition( clientID, 22,value,vrep.simx_opmode_oneshot);
//				vrep.simxPauseCommunication(clientID,false);
//				
//				
//				
//				System.out.println("done " + count + " " + count2 + " " + jointsHandlers.size() + " " + counter);
				
				
				
				
				
				
				
				
				
				
				
				
//			    
//				if(vrep.simxSetJointTargetVelocity(clientID, obectHandle.w, (float) 1, vrep.simx_opmode_oneshot ) == vrep.simx_error_noerror){
//					System.out.println("Success!");
//				}else{
//					System.out.println(obectHandle.w);
//				}
//				
//				// Fetch the newest joint value from the inbox (func. returns immediately (non-blocking)):
////				
////				//
////			    if (vrep.simxGetObjectOrientation(clientID,obectHandle.w,-1,fw,vrep.simx_opmode_streaming)  ==vrep.simx_error_noerror)
////			    { 
////			        // here we have the newest joint position in variable jointPosition! 
////
////				System.out.println("Object Handle: "  + obectHandle.w);
////			    	System.out.println("Angle1: " + fw.w[0]);
////			    	System.out.println("Angle1: " + fw.w[1]);
////			    	System.out.println("Angle1: " + fw.w[2]);
////			    	
////			    	//System.out.println(objectHandle);
////			    	
////			    	
//			    	int ret=vrep.simxGetObjects(clientID,vrep.sim_handle_all,objectHandles,vrep.simx_opmode_oneshot_wait);
//					if (ret==vrep.simx_error_noerror)
//						System.out.format("Number of objects in the scene: %d\n",objectHandles.getArray().length);
//					else
//						System.out.format("Remote API function call returned with error code: %d\n",ret);
////			   }
//				FloatWA angles = new FloatWA(3);
//				angles.w[0] = (float) 1;   //(float) fw.w[0];
//				angles.w[1] = (float) 1;//(float) fw.w[1];
//				angles.w[2] = (float) 0.5;//(float) fw.w[2] ;
//			   // if(vrep.simxSetObjectPosition (clientID,obectHandle.w,-1,angles,vrep.simx_opmode_oneshot ) == vrep.simx_error_noerror){
//			    	//System.out.println("Success!");
//			    	//
//			    ///   }
//			    
//			    
////				if(vrep.simxSetJointPosition(clientID, obectHandle.w, (float)-5, vrep. simx_opmode_streaming) == vrep.simx_error_noerror){
////					System.out.println("Success!");
////				}
			    
			}
////			//simxSetObjectPosition 	
//////			//vrep.simxSetObjectOrientation(clientID, vrep.sim_handle_all, -1, fw, vrep.simx_opmode_continuous) ;
//////			//vrep.simxCloseScene(1, 0);
//////			//vrep.simxPauseSimulation(clientID,vrep.simx_opmode_oneshot); 
//////			//vrep.simxSetObjectPosition 
//		}
//		else
//			System.out.println("Failed connecting to remote API server");
//		System.out.println("Program ended");
//		
	}
//
//	private static void testingg(remoteApi vrep) {
//
//		
	}
}
			
